#include "global.h"

// 初始化卡尔曼滤波器
void kalman_init(_st_KalmanFilter *pKF, float initial_x, float initial_p, float q, float r)
{
    pKF->x = initial_x;
    pKF->p = initial_p;
    pKF->q = q;
    pKF->r = r;
    pKF->k = 0.0f;
}

// 卡尔曼滤波预测步骤
void kalman_predict(_st_KalmanFilter *pKF)
{
    pKF->p = pKF->p + pKF->q;
}

// 卡尔曼滤波更新步骤
void kalman_update(_st_KalmanFilter *pKF, float measurement)
{
    pKF->k = pKF->p / (pKF->p + pKF->r);
    pKF->x = pKF->x + pKF->k * (measurement - pKF->x);
    pKF->p = (1 - pKF->k) * pKF->p;
}

// 初始化低通滤波器
void lowPass_init(_st_LowPassFilter *pLPF, float factor)
{
    pLPF->factor = factor;
    pLPF->x = 0.0f;
}

// 低通滤波更新步骤
void lowPass_update(_st_LowPassFilter *pLPF, float measurement)
{
    pLPF->x = pLPF->x * (1.0 - pLPF->factor) + measurement * pLPF->factor;
}
